#!/usr/bin/env python

import rospy
from math import sin, cos
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
from move_tb2.srv import Destination

#Turtlebot2 move class
class GoToPose():
    def __init__(self):
        
        self.goal_sent = False

        # what to do if shut down (e.g. ctrl + C or failure)
        rospy.on_shutdown(self.shutdown)    
        
        # tell the action client that we want to spin a thread by default
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        rospy.loginfo("wait for the action server to come up")

        # wait for the action server to come up
        self.move_base.wait_for_server()
    
    def goto(self, x, y, theta=0.0, frame="map"):

        # we'll send a goal to the robot to move 
        self.goal_sent = True
        move_goal = MoveBaseGoal()
        move_goal.target_pose.header.frame_id = frame
        move_goal.target_pose.header.stamp = rospy.Time.now()
        move_goal.target_pose.pose.position.x = x
        move_goal.target_pose.pose.position.y = y
        move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
        move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
        
        # use quads to specify the state
        # pos = {'x': -1.0, 'y' : -2.0}
        # quat = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.000, 'r4' : 1.000}
        # move_goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
        #                              Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))

        # start moving
        self.move_base.send_goal(move_goal)

        success = self.move_base.wait_for_result()
        state = self.move_base.get_state()
        
        #result is the flag for indicating whether the goal is reached
        result = 0

        if success and state == GoalStatus.SUCCEEDED:
            rospy.loginfo("success go to (%d, %d)!" %(x,y))  
            result = 1         
        else:
            self.move_base.cancel_goal()
            rospy.loginfo("failed for some reasons!")
                
        self.goal_sent = False
        return result
        
    def shutdown(self):
        if self.goal_sent:
            self.move_base.cancel_goal()
        rospy.loginfo("Stop")

#encapsulate turtlebot2 move class into a service
class DestinationService():
    def __init__(self,navigator):
        self.navigator = navigator

    def call_service(self):
        s = rospy.Service("wait_for_destination", Destination,self.handle_wait_for_destination)
        print("Ready to receive destination.")
        rospy.spin()

    def handle_wait_for_destination(self, req):
        self.x = req.x
        self.y = req.y
        #result is the flag for indicating whether the goal is reached
        result = navigator.goto(self.x, self.y)
        return result


if __name__ == "__main__":
    try:
        rospy.init_node("goto_point", anonymous=False)
        #prepare for going to destination received from service
        navigator = GoToPose()

        #service to receive navigation destination
        dest = DestinationService(navigator)
        dest.call_service()
        
        rospy.sleep(1)

    except rospy.ROSInterruptException:
        rospy.loginfo("Ctrl-C caught. Quitting")
